/*
 * rewarn.cc
 *
 *  Created on: 2009-5-25
 *      Author: tulai
 */

#include "rewarn.h"
#include <math.h>
#include <random.h>

#define INF_TIME 65535
#define BROADCAST_RANGE 200
#define RELAY_COF 1.3
#define CURRENT_TIME    Scheduler::instance().clock()
#define PROB_RELAY 3 //0 always relay, 1 relay with normalized probability, 2 relay with highest probability, 3 fix relay, 4 no relay
#define NAKACONST 8000
#define NAKAEXP 2.718281828

int hdr_rewarn_pkt::offset_;
static class rewarnHeaderClass : public PacketHeaderClass
{
public:
  rewarnHeaderClass() :
    PacketHeaderClass("PacketHeader/rewarn", sizeof(hdr_rewarn_pkt))
  {
    bind_offset(&hdr_rewarn_pkt::offset_);
  }
} class_rtProtorewarn_hdr;

int hdr_rwrelay_pkt::offset_;
static class rwrelayHeaderClass : public PacketHeaderClass
{
public:
  rwrelayHeaderClass() :
    PacketHeaderClass("PacketHeader/rwrelay", sizeof(hdr_rwrelay_pkt))
  {
    bind_offset(&hdr_rwrelay_pkt::offset_);
  }
} class_rtProtorwrelay_hdr;

static class rewarnClass : public TclClass
{
public:
  rewarnClass() :
    TclClass("Agent/rewarn")
  {
  }
  TclObject*
  create(int argc, const char* const * argv)
  {
    assert(argc == 5);
    //return (new rewarn((nsaddr_t) atoi(argv[4])));
    return (new rewarn((nsaddr_t) Address::instance().str2addr(argv[4])));
  }
} class_rtProtorewarn;

// add command
int
rewarn::command(int argc, const char* const * argv)
{
  if (argc == 2)
    {
      Tcl& tcl = Tcl::instance();

      if (strncasecmp(argv[1], "id", 2) == 0)
        {
          tcl.resultf("%d", ra_addr_);
          return TCL_OK;
        }

      if (strncasecmp(argv[1], "start", 2) == 0)
        {
          btimer.handle((Event*) 0);
          return TCL_OK;
        }
    }
  else if (argc == 3)
    {
      if (strcmp(argv[1], "index") == 0)
        {
          ra_addr_ = atoi(argv[2]);
          return TCL_OK;
        }

      else if (strcmp(argv[1], "log-target") == 0 || strcmp(argv[1],
          "tracetarget") == 0)
        {
          logtarget = (Trace*) TclObject::lookup(argv[2]);
          if (logtarget == 0)
            return TCL_ERROR;
          return TCL_OK;
        }
      else if (strcmp(argv[1], "port-dmux") == 0)
        {
          dmux_ = (PortClassifier *) TclObject::lookup(argv[2]);
          if (dmux_ == 0)
            {
              fprintf(stderr, "%s: %s lookup of %s failed\n", __FILE__,
                  argv[1], argv[2]);
              return TCL_ERROR;
            }
          return TCL_OK;
        }
      else if (strcmp(argv[1], "filename") == 0)
        {
          strcpy(filename, argv[2]);
        }
    }
  return Agent::command(argc, argv);
}

// add constructor
rewarn::rewarn(nsaddr_t id) :
  Agent(PT_REWARN), btimer(this), rtimer(this)
{
  ra_addr_ = id;
  node_ = (MobileNode*) Node::get_node_by_address(id);
  nlist_ = (rewarn_nlist*) new (rewarn_nlist);
  rlist_ = (rewarn_rlist*) new (rewarn_rlist);
  WarningCounter = 0;
  //================================================
  //init neighbor list
  //set timer for broadcast
  //================================================
  lstate_.posx = node_->X();
  lstate_.posy = node_->Y();
  //=========================================================
  //=========================================================
  lstate_.velocity = 0;//node_->getcurvel();
  lstate_.yaw = 0; //node_->getcuryaw();
  lstate_.accx = 0; //node_->getaccx();
  lstate_.accy = 0; //node_->getaccx();
  lstate_.ttx = INF_TIME;
  lstate_.tar_addr = -1;
}

void
rewarn::recv(Packet *p, Handler *)
{

  struct hdr_cmn *ch = HDR_CMN(p);
//  struct hdr_ip *ih = HDR_IP(p);

  //assert(initialized());
  //assert(p->incoming == 0);
  // XXXXX NOTE: use of incoming flag has been depracated; In order to track direction of pkt flow, direction_ in hdr_cmn is used instead. see packet.h for details.

  if (ch->ptype() == PT_REWARN)
    {
      recvReWarn(p);
      return;
    }

  else if (ch->ptype() == PT_RWRELAY)
    {
      recvRwRelay(p);
      return;
    }
}

void
rewarn::recvReWarn(Packet *p)
{
  // process when receive vehicle inited rewarn packet
  double ttx = INF_TIME;
  struct hdr_rewarn_pkt *rq = HDR_REWARN_PKT(p);
  int i= nlist_->find_node_by_address(rq->src_addr_);
  if (i != -1)
    nlist_->remove(i);//remove(i) will also cause entries from i+i to the number of entries in the list to move ahead;

  node_state_t n_state_;
  n_state_.timestamp = CURRENT_TIME;
  n_state_.addr_ = rq->src_addr_;
  n_state_.posx = rq->posx;
  n_state_.posy = rq->posy;
  n_state_.velocity = rq->velocity;
  n_state_.yaw = rq->yaw;
  n_state_.accx = rq->accx;
  n_state_.accy = rq->accy;
  n_state_.ttx = rq->ttx;
  n_state_.tar_addr = rq->tar_addr;

  nlist_->insert(n_state_); // inside insert function, should first judge whether list is full, if full, remove(0)


  if (rq->tar_addr != -1)
    {
      if (i = nlist_->find_node_by_address(rq->tar_addr) != -1)
        if(nlist_->get(i)->tar_addr == rq->src_addr_)
          {
          nlist_->get(i)->ttx = rq->ttx;
          rtimer.KillRelayTimer(rq->src_addr_, rq->tar_addr);
          }
    }

  for (u_int16_t i = 0; i < nlist_->getsize() - 1; i++)
    {
      if (nlist_->get(i)->addr_ != rq->tar_addr)
        {
          ttx = getttx(nlist_->get(i), nlist_->get(nlist_->getsize() - 1));
          if (ttx < nlist_->get(i)->ttx)
            {
              nlist_->get(i)->ttx = ttx;
              nlist_->get(i)->tar_addr = rq->src_addr_;
              if (ttx < nlist_->get(nlist_->getsize() - 1)->ttx) {
                nlist_->get(nlist_->getsize() - 1)->ttx = ttx;
                nlist_->get(nlist_->getsize() - 1)->tar_addr = nlist_->get(i)->addr_;
                }
              double abstime, interval;
              abstime = CURRENT_TIME;
              interval = RELAY_COF * BROADCAST_RANGE / 20.0 / 4.0; //node_->speed() / 4; //maybe need modification
              rtimer.SetRelayTimer(rq->src_addr_, nlist_->get(i)->addr_, abstime + interval);
            }
        }
    }

  if(nlist_->getsize() > 0)
    if ((ttx = getttx(&lstate_, nlist_->get(nlist_->getsize() - 1))) < INF_TIME)
     {
      if (lstate_.tar_addr == rq->src_addr_ || ttx <= lstate_.ttx)
        {
          lstate_.tar_addr = rq->src_addr_;
          lstate_.ttx = ttx;
        }
      nlist_->get(nlist_->getsize() - 1)->ttx = ttx;
      nlist_->get(nlist_->getsize() - 1)->tar_addr = ra_addr_;
      sndWarning();
     }
}

void
rewarn::recvRwRelay(Packet *p)
{
  //Process when receiving vehicle relay rewarn packet
  struct hdr_rwrelay_pkt *rq = HDR_RWRELAY_PKT(p);
  node_state_t n_state_;
  int i;
  if (rq->party1 == ra_addr_)
    {
      n_state_.timestamp = rq->timestamp2;
      n_state_.addr_ = rq->party2;
      n_state_.posx = rq->posx2;
      n_state_.posy = rq->posy2;
      n_state_.velocity = rq->velocity2;
      n_state_.yaw = rq->yaw2;
      n_state_.accx = rq->accx2;
      n_state_.accy = rq->accy2;
      n_state_.ttx = rq->ttx;
      n_state_.tar_addr = ra_addr_;
      if (i = nlist_->find_node_by_address(rq->party2) != -1)
        nlist_->remove(i);
      nlist_->insert(n_state_);
      lstate_.tar_addr = rq->party2;
      lstate_.ttx = rq->ttx;
      sndWarning();
    }

  else if (rq->party2 == ra_addr_)
    {
      n_state_.timestamp = rq->timestamp1;
      n_state_.addr_ = rq->party1;
      n_state_.posx = rq->posx1;
      n_state_.posy = rq->posy1;
      n_state_.velocity = rq->velocity1;
      n_state_.yaw = rq->yaw1;
      n_state_.accx = rq->accx1;
      n_state_.accy = rq->accy1;
      n_state_.ttx = rq->ttx;
      n_state_.tar_addr = ra_addr_;
      if (i = nlist_->find_node_by_address(rq->party1) != -1)
        nlist_->remove(i);
      nlist_->insert(n_state_);
      lstate_.tar_addr = rq->party1;
      lstate_.ttx = rq->ttx;
      sndWarning();
    }

}

void
rewarn::sendReWarn()
{
  //update local info
  lstate_.timestamp = CURRENT_TIME;
//===============================================
//update

  lstate_.posx = node_->X();
  lstate_.posy = node_->Y();
  //=========================================================
  //=========================================================
/*  lstate_.velocity = 0;//node_->getcurvel();
  lstate_.yaw = 0; //node_->getcuryaw();
  lstate_.accx = 0; //node_->getaccx();
  lstate_.accy = 0; //node_->getaccx();
  lstate_.ttx = INF＿TIME;
  lstate_.tar_addr = -1;
   */


  for (int i = 0; i < nlist_->getsize(); i++)
    {
      //=================================================
      if (double ttx = getttx(&lstate_, nlist_->get(i)) < INF_TIME )
        { //detect whether local node_ will collide with node i in the list
          if (ttx <= lstate_.ttx)
            {
              lstate_.ttx = ttx;
              lstate_.tar_addr = nlist_->get(i)->addr_;
            }//end if(ttx < node...
          if (nlist_->get(i)->tar_addr == ra_addr_ || ttx
              <= nlist_->get(i)->ttx)
            {
              nlist_->get(i)->timestamp = CURRENT_TIME;
              nlist_->get(i)->tar_addr = ra_addr_;
              nlist_->get(i)->ttx = ttx;
            } //end of if(neighbor ...
          sndWarning(); //send warning indication (warning counter +1 )
        }//end of if(double ttx ...
    }//end of for(int i...

  //send rewarn packet
  Packet *p = Packet::alloc();

  struct hdr_cmn *ch = HDR_CMN(p);
  struct hdr_ip *ih = HDR_IP(p);
  struct hdr_rewarn_pkt *rq = HDR_REWARN_PKT(p);

  ch->ptype() = PT_REWARN;
  // ch->size() = IP_HDR_LEN + rq->size();
  ch->iface() = -2;
  ch->error() = 0;
  ch->addr_type() = NS_AF_NONE;
  ch->prev_hop_ = ra_addr_;

  ih->saddr() = ra_addr_;
  ih->daddr() = IP_BROADCAST;
  ih->sport() = RT_PORT;
  ih->dport() = RT_PORT;
  //       ih->ttl_-=1;

  // Fill up some more fields.
  rq->timestamp = lstate_.timestamp;
  rq->src_addr_ = ra_addr_;
  rq->posx = lstate_.posx;
  rq->posy = lstate_.posy;
  rq->velocity = lstate_.velocity;
  rq->yaw = lstate_.yaw;
  rq->accx = lstate_.accx;
  rq->accy = lstate_.accy;
  rq->ttx = lstate_.ttx;
  rq->tar_addr = lstate_.tar_addr;

  //Scheduler the packet to timer
  Scheduler::instance().schedule(target_, p, 0.);


  return;

}

void
rewarn::sendRwRelay(nsaddr_t n1, nsaddr_t n2)
{
  //send rwrelay packet
  Packet *p = Packet::alloc();
  int i;

  struct hdr_cmn *ch = HDR_CMN(p);
  struct hdr_ip *ih = HDR_IP(p);
  struct hdr_rwrelay_pkt *rq = HDR_RWRELAY_PKT(p);
  bool relayflag = false;
  double prob,sumprob;
  prob = sumprob = 0;

  ch->ptype() = PT_RWRELAY;
  // ch->size() = IP_HDR_LEN + rq->size();
  ch->iface() = -2;
  ch->error() = 0;
  ch->addr_type() = NS_AF_NONE;
  ch->prev_hop_ = ra_addr_;

  ih->saddr() = ra_addr_;
  ih->daddr() = IP_BROADCAST;
  ih->sport() = RT_PORT;
  ih->dport() = RT_PORT;
  //       ih->ttl_-=1;


  if (nlist_->find_node_by_address(n1) == -1 || nlist_->find_node_by_address(n2) == -1)
    //if the two nodes are not in local neighborlist any more, them cancel relay
    {
      rtimer.KillRelayTimer(n1, n2);
      return;
    }
  node_state_t *n_state1, *n_state2;
  n_state1 = nlist_->get(nlist_->find_node_by_address(n1));
  n_state2 = nlist_->get(nlist_->find_node_by_address(n2));

  if(PROB_RELAY == 0){ // all relay
      relayflag = true;
  }
  else if (PROB_RELAY == 1){ //relay with normalized probability
      double tprob;
      double d1,d2;
      //FILE *tfp;
      //tfp = fopen("probabilty.txt", "a");
      //Nakagami distribution
      d1 = dist(node_->X(),node_->Y(),n_state1->posx,n_state1->posy);
      d2 = dist(node_->X(),node_->Y(),n_state2->posx,n_state2->posy);
      prob = pow(NAKAEXP,-d1*d1/NAKACONST)*pow(NAKAEXP,-d2*d2/NAKACONST);
      //fprintf(tfp, "=========================\n relay for node %d and %d \n local node: %d\t, prob:%f\n", n1, n2, ra_addr_, prob);
      for (i = 0; i < nlist_->getsize(); i ++)
        {
          if (nlist_->get(i)->addr_ != n1 && nlist_->get(i)->addr_ != n2)
            {
              d1 = dist(nlist_->get(i)->posx, nlist_->get(i)->posy, n_state1->posx, n_state1->posy);
              d2 = dist(nlist_->get(i)->posx, nlist_->get(i)->posy, n_state2->posx, n_state2->posy);
              tprob = pow(NAKAEXP,-d1*d1/NAKACONST)*pow(NAKAEXP,-d2*d2/NAKACONST);
              //fprintf(tfp, "neighbor node: %d\t, prob:%f\n", nlist_->get(i)->addr_, tprob);
              sumprob += tprob;
            }
        }
      prob = prob/(prob + sumprob);
      double rd = Random::uniform();
      //fprintf(tfp, "rand: %f \t normalized local prob %f\n", rd, prob);
      if(rd < 3*prob)
        relayflag = true;
      else
        relayflag = false;
      //fclose(tfp);
  }
  else if (PROB_RELAY == 2){ //relay with the highest probability
    double d1,d2;
    double tprob;
    d1 = dist(node_->X(),node_->Y(),n_state1->posx,n_state1->posy);
    d2 = dist(node_->X(),node_->Y(),n_state2->posx,n_state2->posy);
    prob = pow(NAKAEXP,-d1*d1/NAKACONST)*pow(NAKAEXP,-d2*d2/NAKACONST);
      for (i = 0; i < nlist_->getsize(); i ++)
        {
          if (nlist_->get(i)->addr_ != n1 && nlist_->get(i)->addr_ != n2)
            {
              d1 = dist(nlist_->get(i)->posx, nlist_->get(i)->posy, n_state1->posx, n_state1->posy);
              d2 = dist(nlist_->get(i)->posx, nlist_->get(i)->posy, n_state2->posx, n_state2->posy);
              tprob = pow(NAKAEXP,-d1*d1/NAKACONST)*pow(NAKAEXP,-d2*d2/NAKACONST);
              if (prob >= tprob){
                relayflag = true;
                continue;
                }
              else{
                relayflag = false;
                break;
              }
            }
        }
   }
  else if (PROB_RELAY == 3){ //only fixed n0 relay
    if (ra_addr_ == 0) relayflag = true;
    else relayflag = false;
  }
  else
    relayflag = false;

  if (relayflag)
  {
    // Fill up some more fields.
    rq->timestamp1 = n_state1->timestamp;
    rq->party1 = n_state1->addr_;
    rq->posx1 = n_state1->posx;
    rq->posy1 = n_state1->posy;
    rq->velocity1 = n_state1->velocity;
    rq->yaw1 = n_state1->yaw;
    rq->accx1 = n_state1->accx;
    rq->accy1 = n_state1->accy;

    rq->timestamp2 = n_state2->timestamp;
    rq->party2 = n_state2->addr_;
    rq->posx2 = n_state2->posx;
    rq->posy2 = n_state2->posy;
    rq->velocity2 = n_state2->velocity;
    rq->yaw2 = n_state2->yaw;
    rq->accx2 = n_state2->accx;
    rq->accy2 = n_state2->accy;

    rq->ttx = n_state1->ttx;

    //Scheduler the packet to timer
    Scheduler::instance().schedule(target_, p, 0.);
  }
  return;
}

//============================================
double
rewarn::getttx(node_state_t *n1, node_state_t *n2)
{
  //if collide
  //return to absolute time to collision
  //that it timestamp + ttx of either node

  //if not collide
  double x1, y1, v1, x2, y2, v2;
  x1 = n1->posx;
  y1 = n1->posy;
  v1 = 20;
  //v1 = n1->velocity;

  x2 = n2->posx;
  y2 = n2->posy;
  v2 = 20;
  //v2 = n2->velocity;

  if (pow(x1-y2, 2) < 0.01  && pow(y1 + x2 -1000.0, 2)<0.01)
    return (500.0 - x1)/20.0;
  if (pow(y1-x2,2) <0.01 && pow(x1 + y2-1000.0, 2)<0.01)
    return (500.0 - y1)/20.0;
  else
    return INF_TIME;

}

void
rewarn::sndWarning()
{
  //send warning indication to high layer application
  //or increase warnnig counter
  //node_->warning_counter_++;
  WarningCounter++;


  FILE *tfp;
  switch(PROB_RELAY)
  {
    case 0:
      tfp = fopen("always_relay.txt", "a");
      break;
    case 1:
      tfp = fopen("prob_relay.txt", "a");
      break;
    case 2:
      tfp = fopen("best_relay.txt", "a");
      break;
    case 3:
      tfp = fopen("fix_relay.txt", "a");
      break;
    case 4:
      tfp = fopen("no_relay.txt", "a");
      break;
    default:
      tfp = fopen("no_relay.txt", "a");
      break;
  }

  fprintf(tfp, " src\t%d \n tar\t%d \n", ra_addr_, lstate_.tar_addr);
  fclose(tfp);
}

void
rewarn::log_rewarn_pkt(){

  sprintf(logtarget->pt_->buffer(), "P\t%d\t%d", 111, 222);
  logtarget->pt_->dump();
}

void
rewarn::log_rwrelay(){

}


double
rewarn::dist(double x1, double y1, double x2, double y2){
  double d1,d2;
  d1 = sqrt((x1 -500)* (x1-500) + (y1 -500)* (y1-500));
  d2 = sqrt((x2 -500)* (x2-500) + (y2 -500)* (y2-500));
  return d1+d2;
}






//============================================
//settimer()
//killtimer()
// add timer handler
void
RewarnTimer::handle(Event*)
{
  BroadcastExpire(); //call fun when timer expired
}

void
RewarnTimer::BroadcastExpire()
{
  agent->sendReWarn();
  double interval = BROADCAST_RANGE / 20.0 / 4.0; //agent->node_->speed() / 4;
  Scheduler::instance().schedule(this, &intr, interval);
}

void
RewarnTimer::SetRewarnTimer()
{

}



void
RelayTimer::handle(Event*)
{
  RelayExpire(); //call fun when timer expired

}

void
RelayTimer::RelayExpire()
{
  double interval;
  if(agent->rlist_->rhead != NULL){
    agent->sendRwRelay(agent->rlist_->rhead->party1, agent->rlist_->rhead->party2);
    //  double abstime = rlist_->rhead->abstime;
    //  interval = rlist_->rhead->interval;
    agent->rlist_->removehead();
  }
  if (agent->rlist_->rhead != NULL)
    {
      interval = agent->rlist_->rhead->abstime - CURRENT_TIME;
      Scheduler::instance().schedule(this, &intr, interval);
    }
}

void
RelayTimer::SetRelayTimer(nsaddr_t n1, nsaddr_t n2, double abstime)
{
  if (agent->rlist_->rhead == NULL)
    {
      agent->rlist_->insert(n1, n2, abstime);
      double interval = abstime - CURRENT_TIME;
      Scheduler::instance().schedule(this, &intr, interval);
    }
  else if (agent->rlist_->find(n1,n2) != -1){
    agent->rlist_->insert(n1, n2, abstime);
  }
}

void
RelayTimer::KillRelayTimer(nsaddr_t n1, nsaddr_t n2)
{
  agent->rlist_->remove(n1, n2);
}


